#ifndef SE2_H
#define SE2_H

#include <Eigen/Dense>
#include "so2.h"

using namespace std;

class SE2 {
public:
	SE2();
	SE2(const double& x, const double& y, const double& t);
	SE2(const SO2& so2, const Eigen::Vector2d& p);
	SE2 Inv();

	SE2 operator +(const SE2& that) const;
	SE2 operator -(const SE2& that) const;

	const Eigen::Vector2d & translation() const {
    	return p_;
  	}

  	Eigen::Vector2d& translation() {
    	return p_;
  	}

  	const SO2& so2() const{
  		return so2_;
  	}
  	
  	SO2& so2() {
  		return so2_;
  	}
private:
	SO2 so2_;
	Eigen::Vector2d p_;
};

#endif
